#include "freertos/FreeRTOS.h" // 引入FreeRTOS；
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"    // 引入FreeRTOS队列；
#include "esp_system.h"        // 引入ESP系统；
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_bt.h"

#include "esp_gap_ble_api.h"  // 引入低功耗蓝牙API；
#include "esp_gatts_api.h"
#include "esp_gatt_defs.h"    // 引入ESP Gatt的定义；
#include "esp_bt_main.h"
#include "gatts_table_creat_demo.h"
#include "mybluetooth.h"

#include <stdio.h>
#include "driver/i2c.h" // 引入I2C通信协议；

#include "HX711.h"
#include "ADS1115.h"

#include "relay.h"
#include "bump.h"
#include "OLED.h"
#include "mydelay.h"

#define APPMAIN_TAG "MAIN"
#define SWITCH_GPIO 40

//----------------------------  设置一些蓝牙相关参数 ------------------------
#define SAMPLE_QUEUE_LEN 1024 // 根据能接受的缓冲长度调整;
#define BLE_TX_INTERVAL_MS 50 // 每 50ms 发送一个大包，可按需调整;
#define BLE_RETRY_DELAY_MS 10
#define BLE_ACCMU_ITEMS 5     // 5帧放在一起形成一个大包；
#define SINGLE_FRAME_LEN 13   // 单条数据的长度；

#ifndef MIN
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif

typedef struct
{
    uint8_t payload[SINGLE_FRAME_LEN];
} sample_frame_t; // 1帧的数据;

typedef struct
{
    uint8_t payload[SINGLE_FRAME_LEN * BLE_ACCMU_ITEMS];
} accmu_frame_t; // 1个大包的数据;

static QueueHandle_t g_sample_queue; // 用于存储数据的队列；

TaskHandle_t *pTask1 = NULL;
TaskHandle_t *pTask2 = NULL;
TaskHandle_t *pTask3 = NULL;
TaskHandle_t *pTask4 = NULL;

// ----------------------------- 设置一些全局信号变量 ------------------------
volatile bool contact_detect_running = 0; // 信号量：接触压力检测在运行中；
volatile bool getval_running = 0;         // 信号量；正式采集正在运行中；
bool notify_flag = false;
bool ble_working = 0;                     // 信号量：蓝牙正在工作；
volatile int switch_state = 0;            // 信号量：单纯用来描述按钮是否按下；
volatile int contact_state = 0;           // 0表示接触弱，1表示接触中，2表示接触强；


// ----------------------------- 定义一次数据任务的起始包和结束包 -------------
uint8_t start_buf[SINGLE_FRAME_LEN] = {0xFF, 0, 0, 0xFA, 0, 0, 0xFA, 0, 0, 0xFA, 0, 0, 0xFE};
uint8_t end_buf[SINGLE_FRAME_LEN] = {0xFF, 0, 0,  0xFC, 0, 0,  0xFC, 0, 0,  0xFC, 0, 0, 0xFE};

int ledBarLength = 0;               // 定义手柄bar的显示范围；
uint16_t send_count = 0;            // 发送数据计数器；
uint32_t threshold = 1000;          // 修改这个值以判断触摸传感器有没有接触到皮肤；
uint32_t pressureHX711BaseLine = 0; // 用于校准压力传感器的示数；
uint16_t heart_rate_handle_table[HRS_IDX_NB];  // 放置一个表用于存储的蓝牙参数；

// ----------------------------- RTOS任务：进行AD采样 ------------------------
static void get_val(void *arg)
{
    sample_frame_t frame;
    memset(frame.payload, 0, SINGLE_FRAME_LEN);
    // 包头+1个气压传感器数值+光学位移传感器数值+接触压力传感器数值+数据发送次数+包尾
    frame.payload[0] = 0xFF;
    frame.payload[3] = 0xFB;
    //frame.payload[6] = 0xFB;
    //frame.payload[9] = 0xFB;
    frame.payload[9] = 0xFE;

    uint16_t temp = 0;
    TickType_t xLastWakeTime;

    while (1)
    {
        xLastWakeTime = xTaskGetTickCount();
        if (getval_running == true)
        {
            frame.payload[1] = (send_count >> 8) & 255;
            frame.payload[2] = send_count & 255;

            bump_temp = ADS1115_ReadVoltage(3);
            frame.payload[4] = (bump_temp >> 8) & 255;
            frame.payload[5] = bump_temp & 255; // 两个字节用来描述气泵的压力；

            temp = ADS1115_ReadVoltage(0);
            frame.payload[6] = (temp >> 8) & 255;
            frame.payload[7] = temp & 255; // 两个字节用来描述光学传感器的位置；

            uint16_t contactPressureADVal = getContactPressureAD();
            frame.payload[8] = (contactPressureADVal >> 8) & 255;
            frame.payload[9] = contactPressureADVal & 255; // 两个字节用来描述接触传感器的压力；

            frame.payload[10] = (send_count >> 8) & 255;
            frame.payload[11] = send_count & 255; // 将发送次数作为两个字节进行发送。

            // 发送数据
            /*
            if (connected&&need_send)
            {
                esp_ble_gatts_set_attr_value(profile_handle_table[IDX_CHAR_VAL_A], 1, (const uint8_t *) buf);
                esp_ble_gatts_send_indicate(heart_rate_profile_tab[0].gatts_if, heart_rate_profile_tab[0].conn_id,
                                            profile_handle_table[IDX_CHAR_VAL_A], sizeof(buf), buf, false);
            }*/
            send_count++;
            if (xQueueSendToBack(g_sample_queue, &frame, pdMS_TO_TICKS(2)) != pdPASS)
            {
                ESP_LOGW(APPMAIN_TAG, "sample queue full");
            }
            //ESP_LOGI(APPMAIN_TAG, "queued frames: %u",
            //        (unsigned)uxQueueMessagesWaiting(g_sample_queue));
        }
        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(8));
    }
}

// -------------------------- freeRTOS任务：led灯条控制任务 ---------------------
static void ledBar_control_task(void *pvParam)
{
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xBlinkInterval = pdMS_TO_TICKS(100); // 闪烁周期 50ms
    // bool current_state = false;

    while (1)
    {
        xLastWakeTime = xTaskGetTickCount();
        OLED_DrawBar(ledBarLength, 3);
        OLED_Update();
        vTaskDelayUntil(&xLastWakeTime, xBlinkInterval);
    }
}

// -------------------------- freeRTOS任务：ble数据收发任务 ---------------------
static void ble_tx_task(void *arg)
{
    sample_frame_t frame;
    accmu_frame_t accmu_frame;
    TickType_t next_wake = xTaskGetTickCount();

    while (1)
    {
        next_wake = xTaskGetTickCount();
        unsigned queueLen = (unsigned)uxQueueMessagesWaiting(g_sample_queue);
        esp_err_t ret;
        if ((queueLen < BLE_ACCMU_ITEMS) && (getval_running)){goto ble_task_end;}      // 如果数据不够一个大包但是采集还在开始，那就先等着数据累积到大包再干； 
        else if (queueLen >= BLE_ACCMU_ITEMS)                                          // 任何时候，如果数据本身是够长度的，那么该发送就发送；
        {
            for (int i = 0; i < BLE_ACCMU_ITEMS; i++)
            {
                if (xQueueReceive(g_sample_queue, &frame, portMAX_DELAY) != pdPASS)
                {continue;}
                memcpy(&(accmu_frame.payload[i * SINGLE_FRAME_LEN]), frame.payload, SINGLE_FRAME_LEN);
            }

            /*ret = esp_ble_gatts_set_attr_value(profile_handle_table[IDX_CHAR_VAL_A],
                                                         BLE_ACCMU_ITEMS * SINGLE_FRAME_LEN,
                                                         accmu_frame.payload);
            */
            ret = esp_ble_gatts_set_attr_value(
                heart_rate_handle_table[IDX_CHAR_VAL_A],
                BLE_ACCMU_ITEMS*SINGLE_FRAME_LEN,
                accmu_frame.payload);

            if (ret != ESP_OK)
            {ESP_LOGW(APPMAIN_TAG, "notify ret=%s", esp_err_to_name(ret));}

            if (ret == ESP_OK && connected)
            {
                /*ret = esp_ble_gatts_send_indicate(
                    heart_rate_profile_tab[0].gatts_if,
                    heart_rate_profile_tab[0].conn_id,
                    profile_handle_table[IDX_CHAR_VAL_A],
                    SINGLE_FRAME_LEN * BLE_ACCMU_ITEMS, accmu_frame.payload, false);
                */
                ret = esp_ble_gatts_send_indicate(
                    heart_rate_profile_tab[0].gatts_if,
                    heart_rate_profile_tab[0].conn_id,
                    heart_rate_handle_table[IDX_CHAR_VAL_A],
                    BLE_ACCMU_ITEMS*SINGLE_FRAME_LEN,
                    accmu_frame.payload,
                    false);
                //ESP_LOGI(APPMAIN_TAG, "QUEUED frames: %u",
                //         (unsigned)uxQueueMessagesWaiting(g_sample_queue));
            }

            // 如果发生拥堵就将数据逐条返回。
            if (ret == ESP_GATT_CONGESTED || ret == ESP_GATT_BUSY) {
                for (int i = BLE_ACCMU_ITEMS-1; i>=0; i--){
                    memcpy(frame.payload, &(accmu_frame.payload[i * SINGLE_FRAME_LEN]), SINGLE_FRAME_LEN);
                    xQueueSendToFront(g_sample_queue, &frame, 0);
                }
                vTaskDelay(pdMS_TO_TICKS(BLE_RETRY_DELAY_MS));
                continue;
            }
        }
        else{                                                                          // 数据本身量不够，而且也不堆积了，那就只能一条一条发了；
            xQueueReceive(g_sample_queue, &frame, portMAX_DELAY);
            /*ret = esp_ble_gatts_set_attr_value(profile_handle_table[IDX_CHAR_VAL_A],
                                               SINGLE_FRAME_LEN,
                                               frame.payload);*/
            ret = esp_ble_gatts_set_attr_value(
                heart_rate_handle_table[IDX_CHAR_VAL_A],
                SINGLE_FRAME_LEN,
                frame.payload);

            if (ret != ESP_OK)
            {
                ESP_LOGW(APPMAIN_TAG, "notify ret=%s", esp_err_to_name(ret));
            }

            if (ret == ESP_OK && connected)
            {
                /*ret = esp_ble_gatts_send_indicate(
                    heart_rate_profile_tab[0].gatts_if,
                    heart_rate_profile_tab[0].conn_id,
                    profile_handle_table[IDX_CHAR_VAL_A],
                    SINGLE_FRAME_LEN, frame.payload, false);*/
                ret = esp_ble_gatts_send_indicate(
                    heart_rate_profile_tab[0].gatts_if,
                    heart_rate_profile_tab[0].conn_id,
                    heart_rate_handle_table[IDX_CHAR_VAL_A],
                    SINGLE_FRAME_LEN,
                    frame.payload,
                    false);
                //ESP_LOGI(APPMAIN_TAG, "QUEUED frames: %u",
                //         (unsigned)uxQueueMessagesWaiting(g_sample_queue));
            }

            // 如果发生拥堵就把本条数据返回；
            if (ret == ESP_GATT_CONGESTED || ret == ESP_GATT_BUSY) {
                xQueueSendToFront(g_sample_queue, &frame, 0);
                vTaskDelay(pdMS_TO_TICKS(BLE_RETRY_DELAY_MS));
                continue;
            }
        }
        

        if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
            ESP_LOGE(APPMAIN_TAG, "BLE send failed: %s", esp_err_to_name(ret));
        }

ble_task_end:
        vTaskDelayUntil(&next_wake, pdMS_TO_TICKS(BLE_TX_INTERVAL_MS));
    }
}


// ------------------------ freeRTOS任务：Contact_Detection ---------------------
void contact_detection(void *pvParam)
{
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xBlinkInterval = pdMS_TO_TICKS(100);

    uint16_t const pressureBaseLineADVal = 49152;
    uint16_t const pressureBaseLineADLowLimit = pressureBaseLineADVal - 16000;  // 低于下限就是压得太深的情况，3000是目前给定的阈值，可以修改；
    uint16_t const pressureBaseLineADHighLimit = pressureBaseLineADVal - 12000; // 高于上限就是压得太浅的情况，1000是目前给定的阈值，可以修改；

    OLED_DrawLine(127 * pressureBaseLineADLowLimit / 70000, 2);
    OLED_DrawLine(127 * pressureBaseLineADHighLimit / 70000, 2);

    while (1)
    {
        xLastWakeTime = xTaskGetTickCount();

        if (contact_detect_running == 0)
        {
            contact_state = 0;
            vTaskDelayUntil(&xLastWakeTime, xBlinkInterval);
            continue;
        }
        uint16_t pressure = getContactPressureAD();

        ESP_LOGI("HX711", "%u", pressure);

        ledBarLength = MIN(127, 127 * (pressure) / (70000));
        if (pressure > pressureBaseLineADLowLimit && pressure <= pressureBaseLineADHighLimit)
            {contact_state = 1;} // 代表接触压力处在合适的状态;
        else if (pressure <= pressureBaseLineADLowLimit)
            {contact_state = 2;} // 代表接触压力过大;
        else{contact_state = 0;} // 代表接触压力过小;
        vTaskDelayUntil(&xLastWakeTime, xBlinkInterval);
    }
}

void init()
{
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << SWITCH_GPIO),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE};
    gpio_config(&io_conf);
}

void app_main(void)
{
    esp_err_t ret;

    init();
    HX711_init();

    i2c_master_init();
    ADS1115_SetGain(GAIN_TWO);
    ADS1115_SetCompQue(COMPQUE_NONE);
    ADS1115_SetCompLat(COMPLAT_NONLAT);
    DADS1115_SetCompPol(COMPPOL_LOW);
    ADS1115_SetCompMode(COMPMODE_TRAD);
    ADS1115_SetRate(RATE_475);

    OLED_Init(OLED_I2C, OLED_ADD, OLED_SCL, OLED_SDA, OLED_SPEED);

    /* Initialize NVS. */
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
    {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));     // 初始化蓝牙
    ble_init();
    delay_ms(500);

    relays_init();
    delay_ms(500);      // 继电器12关,泵开直到检测到压强为-40kpa
    extract();

    // 创建数据发送任务
    g_sample_queue = xQueueCreate(SAMPLE_QUEUE_LEN, sizeof(sample_frame_t));
    configASSERT(g_sample_queue);

    // 创建数据发送任务
    xTaskCreate(contact_detection, "contact_detection", 8192, NULL, 5, pTask1);
    xTaskCreate(ledBar_control_task, "led_control", 8192, NULL, 5, pTask2);
    xTaskCreate(get_val, "get_val", 8192, NULL, 10, pTask3);
    xTaskCreate(ble_tx_task, "ble_tx", 8192, NULL, 6, pTask4);

    ESP_LOGE("1", "1");
    while (1)
    {
        switch_state = gpio_get_level(SWITCH_GPIO);
        if (switch_state == 0)
        {
            delay_ms(20);
            if (switch_state == 0)
            {
                contact_detect_running = 1;
                ESP_LOGI("BUTTON", "pressed");
                getPressureHX711BaseLine(); // 按按钮的时候读取称重传感器的值用于校正, 0位置强行对齐到49152。
                
                // 将采集数据开始包压入栈；
                sample_frame_t frame;
                memcpy(frame.payload, start_buf, SINGLE_FRAME_LEN);
                xQueueSendToBack(g_sample_queue, &frame, pdMS_TO_TICKS(2));
                
                TickType_t start = xTaskGetTickCount();
                TickType_t end = xTaskGetTickCount();
                while (1)
                {
                    end = xTaskGetTickCount();
                    if ((end - start) * portTICK_PERIOD_MS > 60000)
                    {
                        // contact_detect_running = 0;
                        ESP_LOGE("APPMAIN", "TIMEOUT!");
                        goto task_end;
                    }
                    if (contact_state == 1)
                    {
                        delay_ms(20);
                        if (contact_state == 1)
                        {
                            break;
                        }
                    }
                    vTaskDelay(pdMS_TO_TICKS(100));
                }
                send_count = 0;
                TickType_t xLastWakeTime = xTaskGetTickCount();
                getval_running = 1;notify_flag = 1;
                vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(1000));
                xLastWakeTime = xTaskGetTickCount();
                relay_control(1, 0);
                vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(3000));
                xLastWakeTime = xTaskGetTickCount();
                relay_control(1, 1);
                relay_control(2, 0);
                extract2();
                vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(3000));
                relay_control(2, 1);
                getval_running = 0;notify_flag = 0;

                // 将结束包压入栈中。
                vTaskDelay(pdMS_TO_TICKS(40));
                memcpy(frame.payload, end_buf, SINGLE_FRAME_LEN);
                xQueueSendToBack(g_sample_queue, &frame, pdMS_TO_TICKS(2));
            }
        }
    task_end:
        TickType_t xLastWakeTime = xTaskGetTickCount();
        contact_detect_running = 0;
        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500));
        relay_control(1, 1);
        relay_control(2, 1);
        extract();
    }
}
